#include "driver.h"
#include "cyber/common/log.h"


namespace apollo {
namespace drivers {
namespace livox {

#define PACKET_BUFFER_SIZE 512
#define FRAME_BUFFER_SIZE 10

using apollo::drivers::PointCloud;

const int LIVOX_POINT_THRESHOLD_PER_FRAME = 24000;

LivoxDriver* LivoxDriver::self = nullptr;

bool LivoxDriver::Init() {
  if (node_ == nullptr) {
    AERROR << "node is nullptr";
    return false;
  }

  packet_buffer_ =
      std::make_shared<CircularQueueBuffer<LidarPacket>>(PACKET_BUFFER_SIZE);
  frame_buffer_ =
      std::make_shared<CircularQueueBuffer<MixLidarFrame>>(FRAME_BUFFER_SIZE);

  publisher_.Init(node_);

  if (!LivoxLidarSdkInit(FLAGS_livox_sdk_config_path.c_str())) {
    AERROR << "Livox SDK init fail!";
    LivoxLidarSdkUninit();
    return false;
  }

  LivoxLidarSdkVer sdkversion;
  GetLivoxLidarSdkVer(&sdkversion);
  AINFO << "Livox SDK version" << sdkversion.major << "." << sdkversion.minor
          << "." << sdkversion.patch;

  SetLivoxLidarInfoChangeCallback(OnLivoxLidarInfoChange, nullptr);
  SetLivoxLidarPointCloudCallBack(OnLivoxLidarData, nullptr);

  parse_thread_ = std::thread(OnParseLidarData);
  publish_thread_ = std::thread(OnPublishLidarData);


  return true;
}

bool LivoxDriver::init() { 
  self = this;
  return Init(); 
}

void LivoxDriver::OnLivoxLidarInfoChange(const uint32_t handle,
                                                 const LivoxLidarInfo* info,
                                                 void* client_data) {
  uint32_t status =
      SetLivoxLidarWorkMode(handle, kLivoxLidarNormal,
                            LivoxDriver::OnLivoxLidarWorkMode, nullptr);
  AINFO << "Set work mode status: " << status;
  if (self->devices_.find(handle) == self->devices_.end()) {
    self->devices_[handle] = info->lidar_ip;
    AINFO << "Lidar ip: " << self->devices_[handle]
            << " is mounted on handle " << handle;
  }
}

void LivoxDriver::OnLivoxLidarWorkMode(
    livox_status status, uint32_t handle,
    LivoxLidarAsyncControlResponse* response, void* client_data) {
  return;
}

void LivoxDriver::OnLivoxLidarData(uint32_t handle,
                                           const uint8_t dev_type,
                                           LivoxLidarEthernetPacket* data,
                                           void* client_data) {
  if (self->devices_.find(handle) == self->devices_.end()) {
    // AWARN("OnLivoxLidarData handle [{}] is error.", handle);
    return;
  }
  LidarPacket& out = self->livox_packet_buffer_;
  auto ip = self->devices_[handle];
  std::string pos = "none";
  for (const auto& conf : self->conf_.lidar_conf()) {
    if (ip == conf.ip()) {
      pos = conf.location_id();
      out.frame_id = conf.frame_id();
      break;
    }
  }
  if (pos != "none") {
    LivoxPacket& pkt = self->livox_packet_buffer_.livoxPacket;
    memcpy(&pkt, data, sizeof(LivoxLidarEthernetPacket));
    memcpy(pkt.buf, data->data, kMaxLivoxBufferSize);
    uint64_t stamp = GetLivoxPacketTimestamp(&pkt);
    uint64_t sys_time =
        std::chrono::high_resolution_clock::now().time_since_epoch().count();
    if (sys_time - stamp > 40'000'000 && stamp - sys_time > 40'000'000) {
      // AINFO("Livox packet receive: SysTime {} PointCloudTime {}", sys_time,
              //  stamp);
    }
    // out.type = LidarType::LIVOX_MID_360;
    out.stamp = stamp;
    if (pos == "front") {
      out.livoxPos = LivoxLidarPos::FRONT;
    } else if (pos == "left") {
      out.livoxPos = LivoxLidarPos::LEFT;
    } else if (pos == "right") {
      out.livoxPos = LivoxLidarPos::RIGHT;
    } else if (pos == "rear") {
      out.livoxPos = LivoxLidarPos::REAR;
    } else {
      AERROR << "Unknown livox pos: " << pos;
      return;
    }
    self->InsertLivoxData(&out);
  } else {
    AERROR << "Undefined livox ip address: " << ip;
  }
}

uint64_t LivoxDriver::GetLivoxPacketTimestamp(
    const LivoxPacket* raw_packet) {
  uint64_t timestamp;
  memcpy(&timestamp, raw_packet->timestamp, sizeof(timestamp));
  if (raw_packet->time_type == 0) {
    AINFO << "Livox not synced, check PTP status.";
    return timestamp;
  } else if (raw_packet->time_type == 1 || raw_packet->time_type == 2) {
    return timestamp;
  } else {
    AERROR << "Livox time type not valid: time type is "
             << raw_packet->time_type;
    return 0;
  }
}

bool LivoxDriver::InsertLivoxData(LidarPacket* data) {
  if (!packet_buffer_->Insert(data)) {
    AERROR << "Packet buffer full! Discard oldest livox packet.";
    return false;
  }
  return true;
}

bool LivoxDriver::GetLidarPacket(LidarPacket* packet) {
  return packet_buffer_->Get(packet);
}

bool LivoxDriver::InsertRawFrame(
    const std::shared_ptr<MixLidarFrame> frame) {
  if (frame == nullptr) {
    AERROR << "Insert empty frame!";
    return false;
  }
  if (!frame_buffer_->Insert(frame.get())) {
    AINFO << "Frame buffer full! Discard oldest frame.";
    return false;
  }
  return true;
}

bool LivoxDriver::GetRawFrame(std::shared_ptr<MixLidarFrame>& frame) {
  while (true) {
    if (frame_buffer_->Get(frame.get())) {
      return true;
    }
    std::this_thread::sleep_for(std::chrono::nanoseconds(5000000));
  }
}

void LivoxDriver::OnParseLidarData() {
  std::shared_ptr<MixLidarFrame> current_frame_ = std::make_shared<MixLidarFrame>();
  uint64_t frame_interval_ = 1e8;
  uint32_t seq_ = 0;
  while (!self->stop_flag_) {
    if (!self->GetLidarPacket(&self->livox_packet_buffer_)) {
      std::this_thread::sleep_for(std::chrono::nanoseconds(1000000));
      continue;
    }
    // is first frame?
    if (current_frame_->startStamp == 0) {
      current_frame_->startStamp = self->livox_packet_buffer_.stamp -
                                   self->livox_packet_buffer_.stamp % frame_interval_ +
                                   frame_interval_;
      current_frame_->endStamp = current_frame_->startStamp + frame_interval_;
      current_frame_->seq = seq_;
      AINFO << "first frame, start stamp: " << current_frame_->startStamp
              << ", end stamp: " << current_frame_->endStamp;
    }
    // skip too early packets
    if (self->livox_packet_buffer_.stamp < current_frame_->startStamp) {
      continue;
    }
    // send old and create new frame when receive new packet
    if (self->livox_packet_buffer_.stamp > current_frame_->endStamp) {
      self->InsertRawFrame(current_frame_);
      seq_++;
      uint64_t lastStamp = current_frame_->endStamp;
      while (lastStamp + frame_interval_ < self->livox_packet_buffer_.stamp) {
        lastStamp += frame_interval_;
      }
      current_frame_->Clear();
      current_frame_->startStamp = lastStamp;
      current_frame_->endStamp = lastStamp + frame_interval_;
      current_frame_->seq = seq_;
    }
    // parse data for current frame
  //  if ((self->packet_buffer_.type == LidarType::LIVOX_MID_360)) {
      // check front lidar point size
      if (current_frame_->frontCloud->point_size() >
          LIVOX_POINT_THRESHOLD_PER_FRAME) {
        // AERROR(
        //     "Front lidar point size over limit as: {}, at startStamp: {}. "
        //     "Current frame is cleared.",
        //     current_frame_->frontCloud->point_size(),
        //     current_frame_->startStamp);
        uint64_t lastStamp = current_frame_->endStamp;
        current_frame_->Clear();
        current_frame_->startStamp = lastStamp;
        current_frame_->endStamp = lastStamp + frame_interval_;
        current_frame_->seq = seq_;
        continue;
      }
      self->LivoxDataProcess(&self->livox_packet_buffer_, current_frame_);
    // } else {
    //   AERROR << "Unknown lidar type!";
    // }
  }
}

void LivoxDriver::OnPublishLidarData() {
  std::shared_ptr<MixLidarFrame> lidar_frame = std::make_shared<MixLidarFrame>();
  while (!self->stop_flag_) {
    std::this_thread::sleep_for(std::chrono::nanoseconds(10000000));
    if (!self->GetRawFrame(lidar_frame)) {
      // AINFO("lidar motion don't get raw frame.");
      continue;
    }
    self->publisher_.Publish(lidar_frame);
  }
}

void LivoxDriver::LivoxDataProcess(const LidarPacket* packet, std::shared_ptr<MixLidarFrame>& current_frame_) {
  switch (packet->livoxPos) {
    case LivoxLidarPos::FRONT: {
      self->livox_parser_.ParseData(&(packet->livoxPacket),
                              (current_frame_->frontCloud).get());
      auto header = current_frame_->frontCloud->mutable_header();
      header->set_frame_id(packet->frame_id);
      break;
    }
    case LivoxLidarPos::LEFT: {
      self->livox_parser_.ParseData(&(packet->livoxPacket),
                              (current_frame_->leftCloud).get());
      auto header = current_frame_->leftCloud->mutable_header();
      header->set_frame_id(packet->frame_id);
      break;
    }
    case LivoxLidarPos::RIGHT: {
      self->livox_parser_.ParseData(&(packet->livoxPacket),
                              (current_frame_->rightCloud).get());
      auto header = current_frame_->rightCloud->mutable_header();
      header->set_frame_id(packet->frame_id);
      break;
    }
    case LivoxLidarPos::REAR: {
      self->livox_parser_.ParseData(&(packet->livoxPacket),
                              (current_frame_->rearCloud).get());
      auto header = current_frame_->rearCloud->mutable_header();
      header->set_frame_id(packet->frame_id);
      break;
    }
    default:
      AERROR << "Unknown livox lidar pos!";
      break;
  }
}

}  // namespace livox
}  // namespace drivers
}  // namespace apollo


